#ifndef __DENSO_H__
#define __DENSO_H__


// DSInvKine.cpp : rewrite inverse kinematic solver for DENSO in Cpp
// v1.0 (02/09/12): created
// v2.0 (18/09/12): split to cpp, hpp and start coding
// v2.3 (13/05/13): checkOut(Mat&) is upgraded angle = angle*(1-1/(2*PI))
// v2.4 (13/05/13): JTrajPlanner -> correctly update JPose
// v2.5 (22/05/13): getVelJacob is updated to return m and rad


//#include "stdafx.h"

//#include <cv.h>
//#include <cxcore.h>
//#include <highgui.h>

#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>

#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <vector>
//#include "OpenCV/matwork.hpp"
#include "MatWork.hpp"
#include "RS232.h"


using namespace cv;
using namespace std;

const double INF = 999999999999999;

class DENSOrobot
{

public:
  
	struct DHtable
	{
		double a1, a2, a3, a4, a5, a6;
		double d1, d2, d3, d4, d5, d6;
		double alf1, alf2, alf3, alf4, alf5, alf6;
	};

	struct GenericPose // would be either CartesPose or JointPose with 6 double element
	{
		double p1, p2, p3, p4, p5, p6;
	};

	struct CartesPose
	{
		double x, y, z, rol, pit, yaw;
	};

	struct JointPose
	{
		double j1, j2, j3, j4, j5, j6;
	};

	enum OPERATE_MODE
	{ 
		OPER_MODE_JOINT,
		OPER_MODE_XYZ
	};

	enum DENSO_INIT_MODE
	{ 
		INIT_NOMOVE,
		INIT_MOVE_TO_POSE,
		INIT_MOVE_TO_VIA
	};
	
	//static const int CTRL_MODE_JOINT = 0;
	//static const int CTRL_MODE_XYZ = 1;
	static const int CTRL_MODE_JOINT = 0;
	static const int CTRL_MODE_XYZ = 1;

	static Mat rotx(double fi);

	static Mat roty(double fi);

	static Mat rotz(double fi);

	// deprecated by 25/11/14
	int setMode(OPERATE_MODE mode);
	int getMode(OPERATE_MODE &mode);

	// new interface for setMode/getMode
	int setOperMode(OPERATE_MODE mode);
	int getOperMode(OPERATE_MODE &mode);

	int setInitMode(DENSO_INIT_MODE mode);
	int getInitMode(DENSO_INIT_MODE &mode);

	// **** constructors (some to be deprecated):
	//DENSOrobot();  // !!  potential conflict with DENSOrobot(int use_RS232_flg=0)

	// only give default value to the last argument and only in header file
	DENSOrobot(int use_RS232_flg=0);  // !!

	// overloading with set mode and call init move with INIT_MODE
	DENSOrobot(int use_RS232_flg, OPERATE_MODE mode, DENSO_INIT_MODE initMode);

	// overloading with set mode and call init move
	DENSOrobot(int use_RS232_flg, OPERATE_MODE mode);

	// Default initialization of InitPose is JPose
	DENSOrobot(const Mat InitPose, int use_RS232_flg=0);  // !!

	~DENSOrobot();
	// **** end of constructors

	void FKineSolve(const Mat fi, Mat& A);

	// Date created: 20/04/13
	// Description: - work with Radian angles
	//				- returning e1,...,e6; B1,...,B6
	void FKineSolve(const Mat fi, Mat& e, Mat& B, Mat& A);

	void IKineSolve(const Mat fnlpose, Mat& SOL);

	char checkSOL(Mat& joints);	// moved to public (22/05/13)

	int checkOut(Mat& joints);	// moved to public (22/05/13)

	int PresetMove(int movenum);

	// OrgPose = [j1 j2 j3 j4 j5 j6] (rad) or [x y z r p y] (rad & mm) -> this avoids calculating twice Inv Kine
	// DstPose = [j1 j2 j3 j4 j5 j6] (rad) or [x y z r p y] (rad & mm)
	// v2.0 (WORKING 14/10/14) add mode to distinct joint and xyz control mode
	int TrajPlanner(const Mat OrgPose, const Mat DstPose, Mat& BestTraj, int mode);


	int TrajPlanner(const Mat OrgPose, const Mat DestPose, Mat& BestTraj);


	// a higher abstraction of DENSO robot control that moves the robot to target pose regardless of 
	// current operation mode nor given pose type
	int MoveTo(GenericPose pose);

	// Move to current pose
	int MoveToCurPose();


	// Joint Control: check workspace condition and send to RS232
	// DstPose = [j1 j2 j3 j4 j5 j6] (rad)
	int JTrajPlanner(const Mat DstPose);

	// v_ee = J*theta_dot -> velocity screw relates to joint angular velocity
	//void DENSOrobot::getVelJacob(Mat& Jacob);
	void getVelJacob(Mat& Jacob, Mat& A_16);   // A_123456 (tranf mat from ee to base)

	void updateJPose(const Mat JPose_inp);
	
	// v2.0 (14/10/14) update state value directly from caller depending on control mode (joint/xyz)
	int inputPose(const Mat Pose_inp, int mode = CTRL_MODE_JOINT);	

	// 19/11/14 input some via poses directly from caller depending on control mode (joint/xyz)
	//          these poses are used for some specific pre-defined tasks
	int inputPoses(const vector<Mat> Poses_inp, int mode = CTRL_MODE_JOINT);

	// ********** Velocity Control
	void calcJointRate();

	void calcJPose1(const Mat theta_d, const Mat JPose0, Mat& JPose1);
	
	// v2.0 (14/10/14) add mode to distinct joint and xyz control modes
	int calcPose1(const Mat theta_d, const Mat JPose0, Mat& JPose1, int mode);
	
	int VelCtrl(const cv::Mat VelScrew);
	
	
	/**
	* @brief new version of velocity screw control of robot which allows joint/XYZ mode
	* 
	* @param VelScrew control screw
	* @param mode control mode e.g. xyz or joint Defaults to CTRL_MODE_JOINT.
	* @return int
	*/
	int VelCtrl(const Mat VelScrew, int mode);

	
	//void DENSOrobot::calcJointRate();
	// ********** Velocity Control

	int llv2intCoordSys(cv::Mat& Pose, int mode);


	// current robot's state:
	// Pose = [x y z r p y]
	// JPose = [j1 j2 j3 j4 j5 j6]
	// CPose = Pose
	// (14/10/14) added CPose for clearly declaring Cartesian Pose
	Mat Pose, JPose, CPose;

	DHtable DH;

	int use_RS232;

private:

	RS232 serialPort;
	
	OPERATE_MODE operMode;

	DENSO_INIT_MODE initMode;
	
	Mat RPY_DENSO(const Mat fnlpose);

	void cs4Cal_v20(double fi1, double fi2, double fi3, double fi5, double ax, double ay, double az, 
		double& cfi, double& sfi);

	void abCal_v20(double fi1, double fi2, double fi3, double nx, double ny, double nz, double ox, double oy, double oz, 
		double& A, double& B);

	void cs6Cal_v20(double A, double B, double fi5, 
		double& cfi, double& sfi);

	// char DENSOrobot::checkSOL(Mat& joints);   // moved to public (22/05/13)

	int checkOut(double* joints);

	//int DENSOrobot::checkOut(Mat& joints);	// moved to public (22/05/13)

	void DENSOrobot::getDefaultPose();

	void getDefaultDH();
	
	// to deal with Constructor Delegate in current version while it is only supported in C++11
	int InitClass(int use_RS232_flg);

	// InitClass with oper_mode
	int InitClass(int use_RS232_flg, OPERATE_MODE mode);

	// created 18/06/13 -> moved to public 25/06/13
	//int DENSOrobot::llv2intCoordSys(cv::Mat& Pose, int mode);

	vector<Mat> JPoses, CPoses;
};

#endif
